#!/usr/bin/env python
# -*- encoding: UTF-8 -*-
from __future__ import print_function

import sympyrobot
from sympyrobot import rotx,roty,rotz,r2t,Link
from nao_defines import *
from sympy import pi

def RotXYZMatrix(xAngle,yAngle,zAngle):
    Rx = rotx(xAngle)
    Ry = roty(yAngle)
    Rz = rotz(zAngle)
    R = Rx*Ry*Rz
    return r2t(R)

def RotZYXMatrix(zAngle,yAngle,xAngle):
    Rx = rotx(xAngle)
    Ry = roty(yAngle)
    Rz = rotz(zAngle)
    R = Rz*Ry*Rx
    return r2t(R)

class lmRobot(sympyrobot.Robot):
    def __init__(self,name=""):
        super(lmRobot, self).__init__(name=name)

    def fkine(self,q):
        return sympyrobot.fkine(self,q)

    def fkine_val(self,q):
        T = self.fkine(q)
        return sb.evalf(T)

    def ikine(self,q):
        pass

    def jacob0(self,q):
        return sympyrobot.jacob0(self,q)

    def jacob0_val(self,q):
        T = self.jacob0(q)
        return sb.evalf(T)

    def jacobn(self,q):
        return sympyrobot.jacobn(self,q)

    def jacobn_val(self,q):
        T = self.jacobn(q)
        return sb.evalf(T)

from sympy import Symbol
# 单例修饰器
def singleton(cls, *args, **kw):
    instances = {}
    def _singleton():
        if cls not in instances:
            instances[cls] = cls(*args, **kw)
        return instances[cls]
    return _singleton

# 获取符号Nao机器人参数的表示
@singleton
class Get_symbols(object):
    def __init__(self):
        thetas = [(Symbol("theta_%d" % n), n)  for n in range(1,6)]
        for theta, n in thetas:
            self.__dict__["theta%d" % n] = theta
        self.__dict__["theta%d" % n] = Symbol("theta")

    def __getattr__(self, item):
        if item in self.__dict__:
            return self.__dict__[item]
        else:
            self.__dict__[item] = Symbol(item)
            return self.__dict__[item]

    def tihuan(self,S):
        """ 替换规则 """
        return S.subs([(self.UpperArmLength,Symbol("l_1")),
                       (self.ElbowOffsetY,Symbol("l_2")),
                       (self.ShoulderOffsetY,Symbol("l_3")),
                       (self.ShoulderOffsetZ,Symbol("l_4")),
                       (self.HandOffsetX,Symbol("l_5")),
                       (self.HandOffsetZ,Symbol("l_6")),
                       (self.LowerArmLength,Symbol("l_7"))
                        ])
    def evalf(self,S):
        return S.subs([(self.UpperArmLength,UpperArmLength),
                       (self.ElbowOffsetY,ElbowOffsetY),
                       (self.ShoulderOffsetY,ShoulderOffsetY),
                       (self.ShoulderOffsetZ,ShoulderOffsetZ),
                       (self.HandOffsetX,HandOffsetX),
                       (self.HandOffsetZ,HandOffsetZ),
                       (self.LowerArmLength,LowerArmLength)
                        ])

sb = Get_symbols()

class LeftHand(lmRobot):
    def __init__(self,name="LeftHand"):
        super(LeftHand, self).__init__(name=name)
        self.base[1,3] = sb.ShoulderOffsetY
        self.base[2,3] = sb.ShoulderOffsetZ
        L = []
        L.append( Link(A=0,            alpha=-pi/2, D=0, theta=0,    sigma=0, convention=Link.LINK_MDH) )# LShoulderPitch
        L.append( Link(A=0,            alpha=pi/2,  D=0, theta=pi/2, sigma=0, convention=Link.LINK_MDH) )# LShoulderRoll
        L.append( Link(A=ElbowOffsetY, alpha=pi/2,  D=sb.UpperArmLength, theta=0, sigma=0, convention=Link.LINK_MDH) )# LElbowYaw
        L.append( Link(A=0,            alpha=-pi/2, D=0, theta=0,    sigma=0, convention=Link.LINK_MDH) )# LElbowRoll
        L.append( Link(A=0,            alpha=pi/2,  D=0, theta=0,    sigma=0, convention=Link.LINK_MDH) )# LWristYaw
        self.links=L
        self.tool[0,3] = sb.LowerArmLength + sb.HandOffsetX
        self.tool[2,3] = -sb.HandOffsetZ
        self.tool = RotXYZMatrix(-pi/2,0,-pi/2)*self.tool

class RightHand(lmRobot):
    def __init__(self,name="RightHand"):
        super(RightHand, self).__init__(name=name)
        self.base[1,3] = -sb.ShoulderOffsetY # 与LArm不同
        self.base[2,3] = sb.ShoulderOffsetZ
        L = []
        L.append( Link(A=0,            alpha=-pi/2, D=0, theta=0,    sigma=0, convention=Link.LINK_MDH) )# RShoulderPitch
        L.append( Link(A=0,            alpha=pi/2,  D=0, theta=pi/2, sigma=0, convention=Link.LINK_MDH) )# RShoulderRoll
        L.append( Link(A=-sb.ElbowOffsetY, alpha=pi/2,  D=sb.UpperArmLength, theta=0, sigma=0, convention=Link.LINK_MDH) )# RElbowYaw 与LArm不同
        L.append( Link(A=0,            alpha=-pi/2, D=0, theta=0,    sigma=0, convention=Link.LINK_MDH) )# RElbowRoll
        L.append( Link(A=0,            alpha=pi/2,  D=0, theta=0,    sigma=0, convention=Link.LINK_MDH) )# RWristYaw
        self.links=L
        self.tool[0,3] = sb.LowerArmLength + sb.HandOffsetX
        self.tool[2,3] = -sb.HandOffsetZ
        self.tool = RotXYZMatrix(-pi/2,0,-pi/2)*self.tool

class Nao:
    def __init__(self,name):
        self.LeftHand = LeftHand(name=name+"_LeftHand")
        self.RightHand = RightHand(name=name+"_RightHand")

